#ifndef __GS_ALGORITHM_H_
#define __GS_ALGORITHM_H_

#include "fw_interface.h"
#include <stdbool.h>


typedef struct 
{
    float Q_bias;              /*系统中在roll方向上的陀螺仪产生的漂移的过程噪声，可以通过调参获得最优值 */
    float Q_angle;             /*系统中在roll方向上的，陀螺仪在该时刻产生的过程噪声，可以通过调参获得最优值*/
    float Q_gyro;              /*系统中在roll方向上的陀螺仪产生的角度的过程噪声，可以通过调参获得最优值*/
    float R_angle;             /*系统中在roll方向上的陀螺仪产生的角度的测量噪声，用过0.3*/
    float dt;                  /*卡尔曼滤波过程中计算间隔时间*/
    float PP[2][2];            /*卡尔曼滤波过程中，系统的协方差矩阵*/
    float K_0;                 /*卡尔纳增益*/
    float K_1;                 /*卡尔纳增益*/
    float angle;               /*角度*/
}ST_KALMAN_FILTER_PARA;

typedef struct 
{
    int interrupt_sensitivity; /*0-255 */
    int tremos_sensitivity;   /*level 0-2*/
    int measure_range;
    int sample_frequecy;
    int reading_count;
    int interval;
}ST_GSENSOR_DRIVER_PARA;

typedef struct
{
    float acc_x_last;
    float acc_y_last;
    float acc_z_last;

    float acc_xyz; /*总加速度*/
}ST_ACC;

typedef struct
{
    float acc_rate; /*加速度的变化率*/

    float acc_rate_x; /*x方向*/
    float acc_rate_y; /*y方向*/
    
    float acc_rate_xy; /*水平方向的加速度的变化率*/
}ST_RATE_ACC;

void kf_pitch_init(ST_KALMAN_FILTER_PARA * KF_pitch);

void kf_roll_init(ST_KALMAN_FILTER_PARA * KF_roll);

float highPassFilter(float thresh_high_pass, float gyro);

float lowPassFilter(float input, float previousOutput);

bool judge_measure_angle(float acc_xyz);

ST_KALMAN_FILTER_PARA kalman_filter(float angle_m, float v, ST_KALMAN_FILTER_PARA kfp, ST_ACC st_acc);

void read_initial_acc_gyro(struct FW_GSENSOR_INFO_STRU * g_info, struct FW_GYROSCOPE_INFO_STRU * gy_info, ST_GSENSOR_DRIVER_PARA gsensor_driver_para);

void cal_acc_rate(struct FW_GSENSOR_INFO_STRU * g_info, struct FW_GSENSOR_INFO_STRU * kg_info, ST_RATE_ACC * st_rate_acc, ST_ACC * st_acc);

#endif

